
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_gcs.c
  * @author     baiyang
  * @date       2023-7-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <stdint.h>
#include <stdbool.h>

#include <rthw.h>
#include <rtthread.h>
#include <rtdevice.h>


#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
#include <srvrly_events/srvrly_events.h>
/*-----------------------------------macro------------------------------------*/
// id(4 byte) + chan(1 byte) + sysid(1 byte) + compid(1 byte) + msg
#define FMS_GCS_MSG_HEAD_LEN (sizeof(uint32_t) + sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint8_t))
/*----------------------------------typedef-----------------------------------*/
typedef struct {
    uint8_t chan;

    uint8_t sysid;          ///< ID of message sender system/aircraft
    uint8_t compid;         ///< ID of the message sender component

    struct rt_ringbuffer msg_rb;       // id(4 byte) + chan(1 byte) + sysid(1 byte) + compid(1 byte) + msg
    uint8_t msg_rb_pool[1024];

    mavlink_message_t mavlink_msg;

    struct rt_mutex _mutex;
} fms_gcs_msg;

/*---------------------------------prototype----------------------------------*/
static void fms_handle_manual_control_axes(const mavlink_manual_control_t *packet, const uint32_t tnow);
static void fms_rc_manual_override(rc_channel_t c, int16_t value_in, const uint16_t offset, const float scaler, const uint32_t tnow, const bool reversed);
static void fms_handle_rc_channels_override(const mavlink_rc_channels_override_t *packet);
static void fms_handle_set_mode(const mavlink_set_mode_t* set_mode, mavlink_message_t *msg);
static void fms_handle_change_alt_request(mission_command_t cmd);
static void fms_push_msg(uint32_t msg_id, uint8_t chan, uint8_t sysid, uint8_t compid, uint8_t *msg_buf, uint32_t msg_len);
static uint32_t fms_get_msg_head();
static void fms_get_msg_date(uint8_t *msg_buf, uint32_t msg_len);
static void fms_proc_command_long(mavlink_command_long_t* command, mavlink_message_t* msg);
static void fms_proc_command_int(mavlink_command_int_t* command, mavlink_message_t* msg);
static MAV_RESULT fms_handle_command_component_arm_disarm(const mavlink_command_long_t *packet);
static void fms_send_command_ack(uint16_t command, uint8_t result, mavlink_message_t* msg);
static MAV_RESULT fms_handle_command_do_set_mode(const mavlink_command_long_t *packet);
static MAV_RESULT fms_set_mode_common(const MAV_MODE _base_mode, const uint32_t _custom_mode);
static void fms_gcs_cb(void *parameter);
static bool fms_should_handle_message(const mavlink_message_t* msg);
static bool fms_should_handle_command_long(const mavlink_command_long_t* command_long);
static bool fms_should_handle_command_int(mavlink_command_int_t* command_int);
static bool mavlink_coordinate_frame_to_location_alt_frame(const MAV_FRAME coordinate_frame, AltFrame *frame);
static MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t *packet);
static bool location_from_command_t(const mavlink_command_int_t *in, Location *out);
static bool command_long_stores_location(const MAV_CMD command);
/*----------------------------------variable----------------------------------*/
static fms_gcs_msg gcs_msg;

static const float magic_force_arm_value = 2989.0f;
static const float magic_force_disarm_value = 21196.0f;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_gcs_init()
{
    rt_ringbuffer_init(&gcs_msg.msg_rb, gcs_msg.msg_rb_pool, sizeof(gcs_msg.msg_rb_pool));

    rt_mutex_init(&gcs_msg._mutex, "fms_gcs", RT_IPC_FLAG_FIFO);

    itc_subscribe(ITC_ID(mavlink_message),  fms_gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message1), fms_gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message2), fms_gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message3), fms_gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message4), fms_gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message5), fms_gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message6), fms_gcs_cb);
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_gcs_update()
{
    uint32_t msg_id;
    if (rt_ringbuffer_data_len(&gcs_msg.msg_rb) < FMS_GCS_MSG_HEAD_LEN) {
        // 没有要处理的数据，直接返回
        return;
    }

    msg_id = fms_get_msg_head();

    switch (msg_id) {
        case MAVLINK_MSG_ID_COMMAND_LONG: {
            mavlink_command_long_t command;
            fms_get_msg_date((uint8_t *)&command, sizeof(mavlink_command_long_t));

            mavlink_msg_command_long_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &command);
            fms_proc_command_long(&command, &gcs_msg.mavlink_msg);
        } break;

        case MAVLINK_MSG_ID_COMMAND_INT:
        {
            mavlink_command_int_t command;
            fms_get_msg_date((uint8_t *)&command, sizeof(mavlink_command_int_t));

            mavlink_msg_command_int_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &command);

            fms_proc_command_int(&command, &gcs_msg.mavlink_msg);
            break;
        }

        case MAVLINK_MSG_ID_MANUAL_CONTROL:
        {
            if (gcs_msg.sysid != gcs_sysid_my_gcs()) {
                return; // only accept control from our gcs
            }

            mavlink_manual_control_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_manual_control_t));

            if (packet.target != gcs_sysid_this_mav()) {
                break; // only accept control aimed at us
            }

            if (packet.z < 0) { // Copter doesn't do negative thrust
                break;
            }

            uint32_t tnow = time_millis();

            fms_handle_manual_control_axes(&packet, tnow);

            // a manual control message is considered to be a 'heartbeat'
            // from the ground station for failsafe purposes
            gcs_sysid_myggcs_seen(tnow);
            break;
        }
    
        case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
        {
            mavlink_rc_channels_override_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_rc_channels_override_t));

            fms_handle_rc_channels_override(&packet);
            break;
        }

        case MAVLINK_MSG_ID_SET_MODE:
        {
            mavlink_set_mode_t packet;

            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_set_mode_t));
            mavlink_msg_set_mode_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &packet);

            fms_handle_set_mode(&packet, &gcs_msg.mavlink_msg);
            break;
        }

        case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:   // MAV ID: 82
        {
            // Only allow companion computer (or other external controller) to
            // control attitude in GUIDED mode.  We DON'T want external control
            // in e.g., RTL, CICLE. Specifying a single mode for companion
            // computer control is more safe (even more so when using
            // FENCE_ACTION = 4 for geofence failures).
            if (fms.control_mode != (mode_base_t)&fms.mode_guided) { // don't screw up failsafes
                break; 
            }

            mavlink_set_attitude_target_t att_target;
            fms_get_msg_date((uint8_t *)&att_target, sizeof(mavlink_set_attitude_target_t));

            // Mappings: If any of these bits are set, the corresponding input should be ignored.
            // NOTE, when parsing the bits we invert them for easier interpretation but transport has them inverted
            // bit 1: body roll rate
            // bit 2: body pitch rate
            // bit 3: body yaw rate
            // bit 4: unknown
            // bit 5: unknown
            // bit 6: reserved
            // bit 7: throttle
            // bit 8: attitude

            // if not setting all Quaternion values, use _rate flags to indicate which fields.

            // Extract the Euler roll angle from the Quaternion.
            Quat_t q = {att_target.q[0], att_target.q[1], att_target.q[2], att_target.q[3]};

            // NOTE: att_target.type_mask is inverted for easier interpretation
            att_target.type_mask = att_target.type_mask ^ 0xFF;

            uint8_t attitude_mask = att_target.type_mask & 0b10000111; // q plus rpy

            uint32_t now = time_millis();
            if ((attitude_mask & 0b10000001) ||    // partial, including roll
                    (attitude_mask == 0b10000000)) { // all angles
                fms.guided_state.forced_rpy_cd[0] = degrees(quat_get_roll(&q)) * 100.0f;

                // Update timer for external roll to the nav control
                fms.guided_state.last_forced_rpy_ms[0] = now;
            }

            if ((attitude_mask & 0b10000010) ||    // partial, including pitch
                    (attitude_mask == 0b10000000)) { // all angles
                fms.guided_state.forced_rpy_cd[1] = degrees(quat_get_pitch(&q)) * 100.0f;

                // Update timer for external pitch to the nav control
                fms.guided_state.last_forced_rpy_ms[1] = now;
            }

            if ((attitude_mask & 0b10000100) ||    // partial, including yaw
                    (attitude_mask == 0b10000000)) { // all angles
                fms.guided_state.forced_rpy_cd[2] = degrees(quat_get_yaw(&q)) * 100.0f;

                // Update timer for external yaw to the nav control
                fms.guided_state.last_forced_rpy_ms[2] = now;
            }
            if (att_target.type_mask & 0b01000000) { // throttle
                fms.guided_state.forced_throttle = att_target.thrust * 100.0f;

                // Update timer for external throttle
                fms.guided_state.last_forced_throttle_ms = now;
            }

            break;
        }

        case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:     // MAV ID: 84
        {
            // decode packet
            mavlink_set_position_target_local_ned_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_set_position_target_local_ned_t));

            // exit if vehicle is not in Guided mode
            if (fms.control_mode != (mode_base_t)&fms.mode_guided) {
                break;
            }

            // only local moves for now
            if (packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED) {
                break;
            }

            // just do altitude for now
            fms.next_WP_loc.alt += -packet.z*100.0;
            gcs_send_text(MAV_SEVERITY_INFO, "Change alt to %.1f",
                            (double)((fms.next_WP_loc.alt - fms.home->alt)*0.01f));
                break;
            }

        case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:    // MAV ID: 86
        {
            // decode packet
            mavlink_set_position_target_global_int_t pos_target;
            fms_get_msg_date((uint8_t *)&pos_target, sizeof(mavlink_set_position_target_global_int_t));

            // Only want to allow companion computer position control when
            // in a certain mode to avoid inadvertently sending these
            // kinds of commands when the autopilot is responding to problems
            // in modes such as RTL, CIRCLE, etc.  Specifying ONLY one mode
            // for companion computer control is more safe (provided
            // one uses the FENCE_ACTION = 4 (RTL) for geofence failures).
            if (fms.control_mode != (mode_base_t)&fms.mode_guided) {
                //don't screw up failsafes
                break;
            }

            // Unexpectedly, the mask is expecting "ones" for dimensions that should
            // be IGNORNED rather than INCLUDED.  See mavlink documentation of the
            // SET_POSITION_TARGET_GLOBAL_INT message, type_mask field.
            const uint16_t alt_mask = 0b1111111111111011; // (z mask at bit 3)
                
            bool msg_valid = true;
            struct Mission_Command cmd = {0};

            if (pos_target.type_mask & alt_mask)
            {
                cmd.content.location.alt = pos_target.alt * 100;
                cmd.content.location.relative_alt = false;
                cmd.content.location.terrain_alt = false;
                switch (pos_target.coordinate_frame) 
                {
                    case MAV_FRAME_GLOBAL:
                    case MAV_FRAME_GLOBAL_INT:
                        break; //default to MSL altitude
                    case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
                        cmd.content.location.relative_alt = true;
                        break;
                    case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
                        cmd.content.location.relative_alt = true;
                        cmd.content.location.terrain_alt = true;
                        break;
                    default:
                        gcs_send_text(MAV_SEVERITY_WARNING, "Invalid coord frame in SET_POSTION_TARGET_GLOBAL_INT");
                        msg_valid = false;
                        break;
                }    

                if (msg_valid) {
                    fms_handle_change_alt_request(&cmd);
                }
            } // end if alt_mask       
            break;
        }

        case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38
        case MAVLINK_MSG_ID_MISSION_ITEM:           // MAV ID: 39
        case MAVLINK_MSG_ID_MISSION_ITEM_INT:
        case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
        case MAVLINK_MSG_ID_MISSION_REQUEST:
        case MAVLINK_MSG_ID_MISSION_SET_CURRENT:    // MAV ID: 41
        case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:       // MAV ID: 43
        case MAVLINK_MSG_ID_MISSION_COUNT:          // MAV ID: 44
        case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:      // MAV ID: 45
        case MAVLINK_MSG_ID_MISSION_ACK:
        {
            if (msg_id == MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST) {
                mavlink_mission_write_partial_list_t partial_list;
                fms_get_msg_date((uint8_t *)&partial_list, sizeof(mavlink_mission_write_partial_list_t));
                mavlink_msg_mission_write_partial_list_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &partial_list);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_ITEM) {
                mavlink_mission_item_t mission;
                fms_get_msg_date((uint8_t *)&mission, sizeof(mavlink_mission_item_t));
                mavlink_msg_mission_item_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &mission);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_ITEM_INT) {
                mavlink_mission_item_int_t mission_int;
                fms_get_msg_date((uint8_t *)&mission_int, sizeof(mavlink_mission_item_int_t));
                mavlink_msg_mission_item_int_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &mission_int);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_REQUEST_INT) {
                mavlink_mission_request_int_t request_int;
                fms_get_msg_date((uint8_t *)&request_int, sizeof(mavlink_mission_request_int_t));
                mavlink_msg_mission_request_int_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &request_int);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_REQUEST) {
                mavlink_mission_request_t request;
                fms_get_msg_date((uint8_t *)&request, sizeof(mavlink_mission_request_t));
                mavlink_msg_mission_request_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &request);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_SET_CURRENT) {
                mavlink_mission_set_current_t set_current;
                fms_get_msg_date((uint8_t *)&set_current, sizeof(mavlink_mission_set_current_t));
                mavlink_msg_mission_set_current_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &set_current);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) {
                mavlink_mission_request_list_t request_list;
                fms_get_msg_date((uint8_t *)&request_list, sizeof(mavlink_mission_request_list_t));
                mavlink_msg_mission_request_list_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &request_list);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_COUNT) {
                mavlink_mission_count_t count;
                fms_get_msg_date((uint8_t *)&count, sizeof(mavlink_mission_count_t));
                mavlink_msg_mission_count_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &count);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_CLEAR_ALL) {
                mavlink_mission_clear_all_t clear_all;
                fms_get_msg_date((uint8_t *)&clear_all, sizeof(mavlink_mission_clear_all_t));
                mavlink_msg_mission_clear_all_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &clear_all);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_ACK) {
                mavlink_mission_ack_t ack;
                fms_get_msg_date((uint8_t *)&ack, sizeof(mavlink_mission_ack_t));
                mavlink_msg_mission_ack_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &ack);
            }

            fms_handle_common_mission_message(&gcs_msg.mavlink_msg, gcs_msg.chan);
        } break;

        default: {
            gcs_send_text2(gcs_msg.chan, MAV_SEVERITY_INFO, "[fms_gcs_update]: unknown mavlink msg:%u\n", msg_id);
        } break;
    }
}

// this is called on receipt of a MANUAL_CONTROL packet and is
// expected to call manual_override to override RC input on desired
// axes.
static void fms_handle_manual_control_axes(const mavlink_manual_control_t *packet, const uint32_t tnow)
{
    fms_rc_manual_override(fms.channel_roll,     packet->y, 1000, 2000, tnow, false);
    fms_rc_manual_override(fms.channel_pitch,    packet->x, 1000, 2000, tnow, true);
    fms_rc_manual_override(fms.channel_throttle, packet->z, 0,    1000, tnow, false);
    fms_rc_manual_override(fms.channel_rudder,   packet->r, 1000, 2000, tnow, false);
}

static void fms_rc_manual_override(rc_channel_t c, int16_t value_in, const uint16_t offset, const float scaler, const uint32_t tnow, const bool reversed)
{
    if (c == NULL) {
        return;
    }
    int16_t override_value = 0;
    if (value_in != INT16_MAX) {
        const int16_t radio_min = c->radio_min;
        const int16_t radio_max = c->radio_max;
        if (reversed) {
            value_in *= -1;
        }
        override_value = radio_min + (radio_max - radio_min) * (value_in + offset) / scaler;
    }
    rc_set_override(c, override_value, tnow);
}

// allow override of RC channel values for complete GCS
// control of switch position and RC PWM values.
static void fms_handle_rc_channels_override(const mavlink_rc_channels_override_t *packet)
{
    if(packet->target_system != gcs_sysid_my_gcs()) {
        return; // Only accept control from our gcs
    }

    const uint32_t tnow = time_millis();

    const uint16_t override_data[] = {
        packet->chan1_raw,
        packet->chan2_raw,
        packet->chan3_raw,
        packet->chan4_raw,
        packet->chan5_raw,
        packet->chan6_raw,
        packet->chan7_raw,
        packet->chan8_raw,
        packet->chan9_raw,
        packet->chan10_raw,
        packet->chan11_raw,
        packet->chan12_raw,
        packet->chan13_raw,
        packet->chan14_raw,
        packet->chan15_raw,
        packet->chan16_raw
    };

    for (uint8_t i=0; i<8; i++) {
        // Per MAVLink spec a value of UINT16_MAX means to ignore this field.
        if (override_data[i] != UINT16_MAX) {
            rc_set_override(rcs_chan(i), override_data[i], tnow);
        }
    }
    for (uint8_t i=8; i<ARRAY_SIZE(override_data); i++) {
        // Per MAVLink spec a value of zero or UINT16_MAX means to
        // ignore this field.
        if (override_data[i] != 0 && override_data[i] != UINT16_MAX) {
            // per the mavlink spec, a value of UINT16_MAX-1 means
            // return the field to RC radio values:
            const uint16_t value = override_data[i] == (UINT16_MAX-1) ? 0 : override_data[i];
            rc_set_override(rcs_chan(i), value, tnow);
        }
    }

    gcs_sysid_myggcs_seen(tnow);

}

/**
  * @brief       
  * @param[in]   set_mode  
  * @param[in]   msg  
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_handle_set_mode(const mavlink_set_mode_t* set_mode, mavlink_message_t *msg)
{
    const MAV_MODE _base_mode = (MAV_MODE)set_mode->base_mode;
    const uint32_t _custom_mode = set_mode->custom_mode;

    const MAV_RESULT result = fms_set_mode_common(_base_mode, _custom_mode);

    // send ACK or NAK.  Note that this is extraodinarily improper -
    // we are sending a command-ack for a message which is not a
    // command.  The command we are acking (ID=11) doesn't actually
    // exist, but if it did we'd probably be acking something
    // completely unrelated to setting modes.
    if (MAV_HAVE_PAYLOAD_SPACE(gcs_msg.chan, COMMAND_ACK)) {
        mavlink_msg_command_ack_send(gcs_msg.chan, MAVLINK_MSG_ID_SET_MODE, result,
                                     0, 0,
                                     gcs_msg.sysid,
                                     gcs_msg.compid);
    }
}

/*
  handle a request to change current WP altitude. This happens via a
  callback from handle_mission_item()
 */
static void fms_handle_change_alt_request(mission_command_t cmd)
{
    fms.next_WP_loc.alt = cmd->content.location.alt;
    if (cmd->content.location.relative_alt) {
        fms.next_WP_loc.alt += fms.home->alt;
    }
    fms.next_WP_loc.relative_alt = false;
    fms.next_WP_loc.terrain_alt = cmd->content.location.terrain_alt;
    fms_reset_offset_altitude();
}

static void fms_proc_command_long(mavlink_command_long_t* command, mavlink_message_t* msg)
{
    MAV_RESULT result = MAV_RESULT_FAILED;

    switch (command->command) {
    case MAV_CMD_COMPONENT_ARM_DISARM:
        result = fms_handle_command_component_arm_disarm(command);
        break;

    case MAV_CMD_DO_SET_MODE:
        result = fms_handle_command_do_set_mode(command);
        break;

#if HAL_QUADPLANE_ENABLED
    case MAV_CMD_NAV_TAKEOFF: {
        // user takeoff only works with quadplane code for now
        // param7 : altitude [metres]
        float takeoff_alt = command->param7;
        if (plane.quadplane.available() && plane.quadplane.do_user_takeoff(takeoff_alt)) {
            result = MAV_RESULT_ACCEPTED;
        }
        break;
    }
#endif  // HAL_QUADPLANE_ENABLED

#if HAL_QUADPLANE_ENABLED
    case MAV_CMD_DO_MOTOR_TEST:
        // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
        // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
        // param3 : throttle (range depends upon param2)
        // param4 : timeout (in seconds)
        // param5 : motor count (number of motors to test in sequence)
        result = plane.quadplane.mavlink_motor_test_start(gcs_msg.chan,
                                                        (uint8_t)command->param1,
                                                        (uint8_t)command->param2,
                                                        (uint16_t)command->param3,
                                                        command->param4,
                                                        (uint8_t)command->param5);
        break;
    case MAV_CMD_DO_VTOL_TRANSITION:
        if (plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)command->param1)) {
            result = MAV_RESULT_ACCEPTED;
        }
        break;
#endif

    case MAV_CMD_DO_SET_SERVO:
        if (srvrly_do_set_servo(command->param1, command->param2)) {
            result = MAV_RESULT_ACCEPTED;
        }
        break;

    case MAV_CMD_DO_REPEAT_SERVO:
        if (srvrly_do_repeat_servo(command->param1, command->param2, command->param3, command->param4 * 1000)) {
            result = MAV_RESULT_ACCEPTED;
        }
        break;

#if MB_RELAY_ENABLED
    case MAV_CMD_DO_SET_RELAY:
        if (srvrly_do_set_relay(command->param1, command->param2)) {
            result = MAV_RESULT_ACCEPTED;
        }
        break;

    case MAV_CMD_DO_REPEAT_RELAY:
        if (srvrly_do_repeat_relay(command->param1, command->param2, command->param3 * 1000)) {
            result = MAV_RESULT_ACCEPTED;
        }
        break;
#endif

    default:
        break;
    }

    fms_send_command_ack(command->command, result, msg);
}

static void fms_proc_command_int(mavlink_command_int_t* command, mavlink_message_t* msg)
{
    MAV_RESULT result = MAV_RESULT_FAILED;

    switch (command->command) {
    case MAV_CMD_DO_REPOSITION:
        result = handle_command_int_do_reposition(command);
        break;

    default:
        break;
    }

    fms_send_command_ack(command->command, result, msg);
}

static void fms_send_command_ack(uint16_t command, uint8_t result, mavlink_message_t* msg)
{
    if (MAV_HAVE_PAYLOAD_SPACE(gcs_msg.chan, COMMAND_ACK)) {
        mavlink_msg_command_ack_send(gcs_msg.chan, command, result,
                                     0, 0,
                                     msg->sysid,
                                     msg->compid);
    }
}

static MAV_RESULT fms_handle_command_component_arm_disarm(const mavlink_command_long_t *packet)
{
    if (math_flt_equal(packet->param1,1.0f)) {
        if (mb_arming_is_armed()) {
            return MAV_RESULT_ACCEPTED;
        }
        // run pre_arm_checks and arm_checks and display failures
        const bool do_arming_checks = !math_flt_equal(packet->param2,magic_force_arm_value);
        if (mb_arming_arm(ARMING_METHOD_MAVLINK, do_arming_checks)) {
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;
    }
    if (math_flt_zero(packet->param1))  {
        if (!mb_arming_is_armed()) {
            return MAV_RESULT_ACCEPTED;
        }
        const bool forced = math_flt_equal(packet->param2, magic_force_disarm_value);
        // note disarm()'s second parameter is "do_disarm_checks"
        if (mb_arming_disarm(ARMING_METHOD_MAVLINK, !forced)) {
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;
    }

    return MAV_RESULT_UNSUPPORTED;
}

static MAV_RESULT fms_handle_command_do_set_mode(const mavlink_command_long_t *packet)
{
    const MAV_MODE _base_mode = (MAV_MODE)packet->param1;
    const uint32_t _custom_mode = (uint32_t)packet->param2;

    return fms_set_mode_common(_base_mode, _custom_mode);
}

/*
  code common to both SET_MODE mavlink message and command long set_mode msg
*/
static MAV_RESULT fms_set_mode_common(const MAV_MODE _base_mode, const uint32_t _custom_mode)
{
    // only accept custom modes because there is no easy mapping from Mavlink flight modes to AC flight modes
    if ((uint32_t)_base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
        if (!fms_set_mode2(_custom_mode, MODE_REASON_GCS_COMMAND)) {
            // often we should be returning DENIED rather than FAILED
            // here.  Perhaps a "has_mode" callback on AP_::vehicle()
            // would do?
            return MAV_RESULT_FAILED;
        }
        return MAV_RESULT_ACCEPTED;
    }

    if (_base_mode == (MAV_MODE)MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
        // set the safety switch position. Must be in a command by itself
        if (_custom_mode == 0) {
            // turn safety off (pwm outputs flow to the motors)
            //hal.rcout->force_safety_off();
            return MAV_RESULT_ACCEPTED;
        }
        if (_custom_mode == 1) {

            // turn safety on (no pwm outputs to the motors)
            if (srv_hal_force_safety_on()) {
                return MAV_RESULT_ACCEPTED;
            }
            return MAV_RESULT_FAILED;
        }
        return MAV_RESULT_DENIED;
    }

    // Command is invalid (is supported but has invalid parameters)
    return MAV_RESULT_DENIED;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_gcs_cb(void *parameter)
{
    uitc_mavlink_message *mavlink_msg = (uitc_mavlink_message *)parameter;

    if (!fms_should_handle_message(mavlink_msg->msg_t)) {
        return;
    }

    switch (mavlink_msg->msg_t->msgid) {
        case MAVLINK_MSG_ID_COMMAND_LONG: {
            mavlink_command_long_t command;
            mavlink_msg_command_long_decode(mavlink_msg->msg_t, &command);

            fms_push_msg(MAVLINK_MSG_ID_COMMAND_LONG, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&command, sizeof(mavlink_command_long_t));
        } break;

        case MAVLINK_MSG_ID_COMMAND_INT:
        {
            mavlink_command_int_t command;
            mavlink_msg_command_int_decode(mavlink_msg->msg_t, &command);

            fms_push_msg(MAVLINK_MSG_ID_COMMAND_INT, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&command, sizeof(mavlink_command_int_t));
            break;
        }

        case MAVLINK_MSG_ID_MANUAL_CONTROL:
        {
            mavlink_manual_control_t packet;
            mavlink_msg_manual_control_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MANUAL_CONTROL, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_manual_control_t));
            break;
        }
    
        case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
        {
            mavlink_rc_channels_override_t packet;
            mavlink_msg_rc_channels_override_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_rc_channels_override_t));
            break;
        }

        case MAVLINK_MSG_ID_SET_MODE:
        {
            mavlink_set_mode_t packet;
            mavlink_msg_set_mode_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_SET_MODE, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_set_mode_t));
            break;
        }

        case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:   // MAV ID: 82
        {
            mavlink_set_attitude_target_t packet;
            mavlink_msg_set_attitude_target_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_set_attitude_target_t));
            break;
        }

        case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:     // MAV ID: 84
        {
            // decode packet
            mavlink_set_position_target_local_ned_t packet;
            mavlink_msg_set_position_target_local_ned_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_set_position_target_local_ned_t));
            break;
        }

        case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:    // MAV ID: 86
        {
            // decode packet
            mavlink_set_position_target_global_int_t packet;
            mavlink_msg_set_position_target_global_int_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_set_position_target_global_int_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38
        {
            // decode packet
            mavlink_mission_write_partial_list_t packet;
            mavlink_msg_mission_write_partial_list_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_write_partial_list_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_ITEM:    // MAV ID: 39
        {
            // decode packet
            mavlink_mission_item_t packet;
            mavlink_msg_mission_item_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_ITEM, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_item_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_ITEM_INT:    // MAV ID: 73
        {
            // decode packet
            mavlink_mission_item_int_t packet;
            mavlink_msg_mission_item_int_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_ITEM_INT, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_item_int_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
        {
            // decode packet
            mavlink_mission_request_int_t packet;
            mavlink_msg_mission_request_int_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_REQUEST_INT, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_request_int_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_REQUEST:
        {
            // decode packet
            mavlink_mission_request_t packet;
            mavlink_msg_mission_request_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_REQUEST, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_request_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_SET_CURRENT:    // MAV ID: 41
        {
            // decode packet
            mavlink_mission_set_current_t packet;
            mavlink_msg_mission_set_current_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_SET_CURRENT, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_set_current_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:       // MAV ID: 43
        {
            // decode packet
            mavlink_mission_request_list_t packet;
            mavlink_msg_mission_request_list_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_REQUEST_LIST, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_request_list_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_COUNT:          // MAV ID: 44
        {
            // decode packet
            mavlink_mission_count_t packet;
            mavlink_msg_mission_count_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_COUNT, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_count_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:      // MAV ID: 45
        {
            // decode packet
            mavlink_mission_clear_all_t packet;
            mavlink_msg_mission_clear_all_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_CLEAR_ALL, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_clear_all_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_ACK:
        {
            // decode packet
            mavlink_mission_ack_t packet;
            mavlink_msg_mission_ack_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_ACK, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_ack_t));
            break;
        }

        default: {
            gcs_send_text2(mavlink_msg->chan, MAV_SEVERITY_INFO, "[fms_gcs_cb]: unknown mavlink msg:%u\n", mavlink_msg->msg_t->msgid);
        } break;
    }
}

/**
  * @brief       
  * @param[in]   msg_id  
  * @param[in]   msg_buf  
  * @param[in]   msg_len  
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_push_msg(uint32_t msg_id, uint8_t chan, uint8_t sysid, uint8_t compid, uint8_t *msg_buf, uint32_t msg_len)
{
    if (rt_ringbuffer_space_len(&gcs_msg.msg_rb) >= msg_len + FMS_GCS_MSG_HEAD_LEN) {

        rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);

        rt_ringbuffer_put(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, chan);
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, sysid);
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, compid);
        rt_ringbuffer_put(&gcs_msg.msg_rb, msg_buf, msg_len);

        rt_mutex_release(&gcs_msg._mutex);
    } else {
        rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
        rt_ringbuffer_reset(&gcs_msg.msg_rb);

        rt_ringbuffer_put(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, chan);
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, sysid);
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, compid);
        rt_ringbuffer_put(&gcs_msg.msg_rb, msg_buf, msg_len);

        rt_mutex_release(&gcs_msg._mutex);

        gcs_send_text2(chan, MAV_SEVERITY_INFO, "fms_gcs miss msg");
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static uint32_t fms_get_msg_head()
{
    uint32_t msg_id;
    rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
    rt_ringbuffer_get(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.chan);
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.sysid);
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.compid);
    rt_mutex_release(&gcs_msg._mutex);

    return msg_id;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_get_msg_date(uint8_t *msg_buf, uint32_t msg_len)
{
    rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
    rt_ringbuffer_get(&gcs_msg.msg_rb, msg_buf, msg_len);
    rt_mutex_release(&gcs_msg._mutex);
}

static bool fms_should_handle_message(const mavlink_message_t* msg)
{
    bool res = false;

    switch (msg->msgid) {
        case MAVLINK_MSG_ID_COMMAND_LONG:
        {
            mavlink_command_long_t command;
            mavlink_msg_command_long_decode(msg, &command);

            res = fms_should_handle_command_long(&command);
            break;
        }
        case MAVLINK_MSG_ID_COMMAND_INT:
        {
            mavlink_command_int_t command;
            mavlink_msg_command_int_decode(msg, &command);

            res = fms_should_handle_command_int(&command);
            break;
        }

        case MAVLINK_MSG_ID_MANUAL_CONTROL:
        case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
        case MAVLINK_MSG_ID_SET_MODE:
        case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
        case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
        case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
        case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
        case MAVLINK_MSG_ID_MISSION_ITEM:
        case MAVLINK_MSG_ID_MISSION_ITEM_INT:
        case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
        case MAVLINK_MSG_ID_MISSION_REQUEST:
        case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
        case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
        case MAVLINK_MSG_ID_MISSION_COUNT:
        case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
        case MAVLINK_MSG_ID_MISSION_ACK:
            res = true;
            break;
        default:
            break;
    }

    return res;
}

static bool fms_should_handle_command_long(const mavlink_command_long_t* command_long)
{
    bool res = false;

    switch (command_long->command) {
    case MAV_CMD_COMPONENT_ARM_DISARM:
    case MAV_CMD_DO_SET_MODE:
    case MAV_CMD_NAV_TAKEOFF:
    case MAV_CMD_DO_MOTOR_TEST:
    case MAV_CMD_DO_SET_SERVO:
    case MAV_CMD_DO_SET_RELAY:
    case MAV_CMD_DO_REPEAT_SERVO:
    case MAV_CMD_DO_REPEAT_RELAY:
        res = true;
        break;
    default:
        break;
    }

    return res;
}

static bool fms_should_handle_command_int(mavlink_command_int_t* command_int)
{
    bool res = false;

    switch (command_int->command) {
    case MAV_CMD_DO_REPOSITION:
        res = true;
        break;
    default:
        break;
    }

    return res;
}

static bool mavlink_coordinate_frame_to_location_alt_frame(const MAV_FRAME coordinate_frame, AltFrame *frame)
{
    switch (coordinate_frame) {
    case MAV_FRAME_GLOBAL_RELATIVE_ALT: // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INT
    case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
        *frame = ALT_FRAME_ABOVE_HOME;
        return true;
    case MAV_FRAME_GLOBAL_TERRAIN_ALT:
    case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
        *frame = ALT_FRAME_ABOVE_TERRAIN;
        return true;
    case MAV_FRAME_GLOBAL:
    case MAV_FRAME_GLOBAL_INT:
        *frame = ALT_FRAME_ABSOLUTE;
        return true;
    default:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        gcs_send_text2(gcs_msg.chan, MAV_SEVERITY_INFO, "Unknown mavlink coordinate frame %u", coordinate_frame);
#endif
        return false;
    }
}

static MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t *packet)
{
    (void)packet;
    return MAV_RESULT_ACCEPTED;
}

static bool location_from_command_t(const mavlink_command_int_t *in, Location *out)
{
    if (!command_long_stores_location((MAV_CMD)in->command)) {
        return false;
    }

    // integer storage imposes limits on the altitudes we can accept:
    if (fabsf(in->z) > LOCATION_ALT_MAX_M) {
        return false;
    }

    AltFrame frame;
    if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)in->frame, &frame)) {
        // unknown coordinate frame
        return false;
    }

    out->lat = in->x;
    out->lng = in->y;

    location_set_alt_cm(out, (int32_t)(in->z * 100), frame);

    return true;
}

static bool command_long_stores_location(const MAV_CMD command)
{
    switch(command) {
    case MAV_CMD_DO_SET_HOME:
    case MAV_CMD_DO_SET_ROI:
    case MAV_CMD_DO_SET_ROI_LOCATION:
    case MAV_CMD_NAV_TAKEOFF:
    case MAV_CMD_DO_REPOSITION:
        return true;
    default:
        return false;
    }
    return false;
}
/*------------------------------------test------------------------------------*/

